#include "task_chassis.h"
#include "app.hpp"

[[noreturn]] void task_chassis_entry(void const *argument) {
    while (true) {
        if (dj6.is_connected == false) {
            chassis.SetEnable(false); // 底盘失能，关闭电机输出
            chassis.Update();
            osDelay(1);
            continue;
        }

        // 遥控器指令
        const float vx = dj6.x * settings.vxy_max;
        const float vy = dj6.y * settings.vxy_max;
        const float vr_rpm = dj6.yaw * settings.rpm_max;

        // 控制底盘
        chassis.SetEnable(true); // 使能
        chassis.SetPowerLimit(65535);
        chassis.SetSpeed(vx, vy, vr_rpm);
        chassis.Update();
        osDelay(1);
    }
}
